import rospy
from std_srvs.srv import Empty
from sensor_msgs.msg import LaserScan
import numpy as np
import math

RESOLUTION = 2*np.pi/1947

class Rplidar_Subscriber():
    INSTALL_ANGLE_BIAS = 90
    

    def __init__(self):
        self._scan = None

        rospy.Subscriber(
            "/scan",
            LaserScan,
            self.callback
        )


    def callback(self, data: LaserScan):
        self._scan = data
        # print(len(self._scan.ranges))



    @property
    def ladar_data(self):
        '''get fixed scan data'''
        if(self._scan):
            
            return self._fix(self._scan.ranges, RESOLUTION, self.INSTALL_ANGLE_BIAS) * 100
        else:
            return None

    def _fix(self, ranges, inc_rad, offset = INSTALL_ANGLE_BIAS):
        index = int(offset * math.pi / 180 / inc_rad)
        range_fixed = ranges[index:] + ranges[:index]
        return np.array(range_fixed)

    def start_motor(self):
        rospy.wait_for_service('start_motor')
        s = rospy.ServiceProxy('start_motor', Empty)
        s.call()
        s.close()

    def stop_motor(self):
        rospy.wait_for_service('stop_motor')
        s = rospy.ServiceProxy('stop_motor', Empty)
        s.call()
        s.close()


if __name__ == "__main__":
    rospy.init_node("ladar_test")
    ladar = Rplidar_Subscriber()

    rospy.spin()